package behavior;

import sensor.MainColorSensorB;
import lejos.nxt.MotorPort;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.addon.CompassHTSensor;
import lejos.nxt.comm.RConsole;
import lejos.robotics.subsumption.Behavior;
import lejos.util.Delay;
import motor.carrito;

public class detectBall implements Behavior{
	 	private MainColorSensorB color;
	    private carrito carro;
	    private boolean suppressed = false;
	    private CompassHTSensor compass;
	    private int cid;
	    NXTRegulatedMotor pala;

	    public detectBall(MainColorSensorB color, carrito carro, CompassHTSensor compass )
	    {
	       this.color = color;
	       this.carro = carro;
	       this.compass = compass;
	       this.pala  = new NXTRegulatedMotor(MotorPort.A);
	       
	    }

	    public boolean takeControl() {
	    	
	    	cid = color.reconocer();
	    	if ((cid == 0)||(cid == 1)){	    		
	    		RConsole.println("Tome control de DETECTBALL: " +Integer.toString(cid));
	    		return true;
	    	}
	    	else{
	    		RConsole.println("No reconoci PELOTA: "+Integer.toString(cid));
	    		return false;
	    	}
	       
	    }

	    public void suppress() {
	    	RConsole.println("Suprimi DETECTBALL");
	    	suppressed = true;	       
	    }

	    public void action() {
	    	RConsole.println("Accion DETECTBALL");
	    	suppressed = false;
	    	
	    	if(!suppressed) {
	    		if (cid == 0){  //pelota azul
	    			pala.setSpeed(100);
	    			pala.setAcceleration(100);
	    			pala.resetTachoCount();
	    		}else{         //pelota naranja
	    			pala.setSpeed(1100);
	    			pala.setAcceleration(6000);
	    			pala.resetTachoCount();
	    		}
	    		
	    	/*	float pos = compass.getDegrees();
	    		float inicio = pos;
		    	float fin = 0;
		    	carro.Spin();
		    	if (pos > fin){
		    		while (pos >= fin)
		    			pos = compass.getDegrees();
		    	}    	
		    	while (pos <= fin)    		
		    		pos = compass.getDegrees();
		    	carro.Stop();
	    		*/
	    		pala.rotate(-60,false);
				//Delay.msDelay(2000);
				pala.setAcceleration(100);
				pala.setSpeed(100);
				pala.rotate(60,false);
				
				/*carro.Spin();
		    	if (pos > inicio){
		    		while (pos >= inicio)
		    			pos = compass.getDegrees();
		    	}    	
		    	while (pos <= inicio)    		
		    		pos = compass.getDegrees();
		    	carro.Stop();*/
	    	}
      
	    }

}
